#include "locomotives.hpp"
#include "common.h"
#include "electric_motor.hpp"
// #include "environment.h"
#include <cassert>
#include <cmath>
#include <cstdlib>
fp_type LocomotiveProperty::getMotorTorqueTractionLimit() {
    return this->motorProperty->getTorqueLimitOnAxle(this->wheelDiameter) *
           this->motorNum;
}

Locomotive::~Locomotive() {
    // 默认析构函数
}

void Locomotive::initialize() {
    // 初始化函数
}

fp_type Locomotive::traction(fp_type power, fp_type v, fp_type w_i) {
    // 计算机车的最大牵引力
    fp_type max_adhesion_traction = tractionLimitAdhesion(v, w_i);
    if (v <= this->props->startSpeed) {
        return max_adhesion_traction;
    }

    fp_type max_power_traction = tractionLimitPower(power, v);
    return std::min(max_adhesion_traction, max_power_traction);
}

fp_type Locomotive::tractionLimitAdhesion(fp_type v, fp_type w_i) {
    fp_type mu = adhesion_coefficient(v);
    fp_type slope_coe = std::cos(std::atan(0.001 * w_i));
    fp_type max_adhesion_traction =
        mu * this->adhesionMass() * 9.81 * slope_coe;
    return max_adhesion_traction;
}

fp_type Locomotive::tractionLimitPower(fp_type power, fp_type v) {
    return power / (v / 3.6);
}

fp_type Locomotive::tractionLimitTorque() {
    if (this->props->type == LOCOMOTIVE_TYPE_ELECTRIC) {
        ElectricLocomotive *electricLocomotive = (ElectricLocomotive *)this;
        return electricLocomotive->props->getMotorTorqueTractionLimit();
    } else {
        assert(false);
    }
    // return ElectricMotorProperty_getTorqueLimitOnAxle(&this->motorProperty,
    // wheelDiameter) * motorNum;
}

fp_type Locomotive::speedLimitRPM() {
    switch (props->type) {
    case LOCOMOTIVE_TYPE_ELECTRIC:
        // ElectricLocomotive *electricLocomotive = (ElectricLocomotive *)this;
        // return electricLocomotive->getSpeedLimitRPM();
        assert(false);
        return 0;
        // return ElectricMotorProperty_getMaxPerimeterSpeed(&motorProperty,
        // wheelDiameter);
    default:
        return 999999.0; // 返回一个很大的数据，表示没有速度限制
    }
}

fp_type Locomotive::friction(fp_type v, fp_type w_i) {
    return (2.40 + 0.0022 * v + 0.000391 * v * v + w_i) *
           (this->adhesionMass() * 9.81) / 1000;
}

fp_type Locomotive::adhesion_coefficient(fp_type v) {
    if (v < this->props->startSpeed) {
        v = this->props->startSpeed;
    }
    return 0.4082 - 0.0021 * v;
}

ElectricLocomotive::ElectricLocomotive(int id, const std::string name,
                                       LocomotiveProperty *props)
    : Locomotive(id, name, props) {
    assert(props->type == LOCOMOTIVE_TYPE_ELECTRIC);
    this->motors = ElectricMotor::createMultipleMotors(this->props->motorNum);
    this->updateParameters();
}

ElectricLocomotive::~ElectricLocomotive() {
    // TODO: delete motors
}

void ElectricLocomotive::updateParameters() {
    maxPower = 0.0;
    for (auto &motor : motors) {
        maxPower += this->props->motorProperty->getMaxPower();
    }
}
